﻿// Inner Fire 游戏引擎库
// PhysicsMath - 物理数学
//
// Copyright (c) 2025 by 尔西大帝. All rights reserved.
//
// 所属层级：应用层（4）
// 与物理相关的数学
//
// Author: 尔西大帝
// Email: 2670613871@qq.com
// Created: 2025-06-17

#include "physics_math.h"
#include <game/physics/rigid_body.h>
#include <graphics/imath.h>

using namespace DirectX;

namespace ifire::game {
IVec3 physics_math::GenerateGravitationForce(
    const RigidBody& a, const RigidBody& b, float G, float min, float max) {
  // 得到距离（的平方）
  auto d = XMLoadFloat3(&a.position_) - XMLoadFloat3(&b.position_);
  auto dist_sq = XMVectorGetX(XMVector3LengthSq(d));
  // 根据距离进行裁剪
  dist_sq = imath::Clamp(dist_sq, min, max);

  // 得到方向
  auto dir = XMVector3Normalize(d);
  // 计算引力的大小
  auto mag = G * (a.mass_ * b.mass_) / dist_sq;

  // 方向乘大小，就是引力。
  return StoreF3(dir * mag);
}
IVec3 physics_math::CreateSpringForce(
    const RigidBody& p, IVec3 anchor, float rest_length, float k) {
  // 得到Anchor和粒子的距离
  auto d = XMLoadFloat3(&p.position_) - XMLoadFloat3(&anchor);
  // 得到静止距离和当前拉伸距离的差
  auto displacement = XMVectorGetX(XMVector3Length(d)) - rest_length;

  // 得到方向和强度（这里应该可以再省一次开方计算）
  auto dir = XMVector3Normalize(d);
  // 同样，K为负是因为方向相反
  float mag = -k * displacement;

  // 返回方向*强度
  return StoreF3(dir * mag);
}

IVec3 physics_math::CreateSpringForce(
    const RigidBody& a, const RigidBody& b, float rest_length, float k) {
  return CreateSpringForce(a, b.position_, rest_length, k);
}

// 计算从一个方向向量旋转到另一个方向向量所需的四元数
static XMVECTOR QuaternionRotationFromTo(XMVECTOR from, XMVECTOR to) {
  // 归一化输入向量
  from = XMVector3Normalize(from);
  to = XMVector3Normalize(to);

  // 计算旋转轴(使用叉积)和旋转角度(使用点积)
  XMVECTOR axis = XMVector3Cross(from, to);
  float dot = XMVectorGetX(XMVector3Dot(from, to));

  // 处理平行向量的情况
  if (XMVector3Equal(axis, XMVectorZero())) {
    // 如果方向相同，返回单位四元数
    if (dot > 0.0f) {
      return XMQuaternionIdentity();
    }
    // 如果方向相反，绕任意轴旋转180度(这里选择X轴)
    else {
      return XMQuaternionRotationAxis(
          XMVectorSet(1.0f, 0.0f, 0.0f, 0.0f), XM_PI);
    }
  }

  axis = XMVector3Normalize(axis);
  float angle = acosf(dot);

  return XMQuaternionRotationAxis(axis, angle);
}

Trans3 physics_math::CalcSpringTransform(IVec3 start, IVec3 end, float width) {
  // 1. 计算位置 - 弹簧的中心点
  auto start_vec = XMLoadFloat3(&start);
  auto end_vec = XMLoadFloat3(&end);
  XMVECTOR position = (start_vec + end_vec) * 0.5f;

  // 2. 计算方向向量并确定缩放
  XMVECTOR direction = end_vec - start_vec;
  float length = XMVectorGetX(XMVector3Length(direction));

  // 弹簧的Y轴缩放就是两点之间的距离
  // 假设原始弹簧高度为1单位，所以缩放系数就是实际长度
  XMVECTOR scale = XMVectorSet(width, length, width, 0.0f);

  // 3. 计算旋转
  // 原始弹簧朝向Y轴正方向，我们需要旋转到实际方向
  XMVECTOR defaultDir = XMVectorSet(0.0f, 1.0f, 0.0f, 0.0f);
  XMVECTOR rotation = XMQuaternionIdentity();

  // 如果方向不是零向量(即两点不重合)
  if (length > 0.0001f) {
    direction = XMVector3Normalize(direction);
    rotation = QuaternionRotationFromTo(defaultDir, direction);
  }

  // 4. 返回变换
  return Trans3(position, rotation, scale);
}

IVec physics_math::ClosestPointOnSegmentToAABB(
    IVec segStart, IVec segEnd, IVec aabbMin, IVec aabbMax) {
  // 参数化线段
  IVec dir = segEnd - segStart;
  float length = XMVectorGetX(XMVector3Length(dir));
  if (length < FLT_EPSILON) return segStart;

  IVec unitDir = dir / length;
  float t = 0.5f; // 初始中点

  // 投影到各轴
  for (int i = 0; i < 3; ++i) {
    if (fabs(XMVectorGetByIndex(unitDir, i)) > FLT_EPSILON) {
      float tMin =
          (XMVectorGetByIndex(aabbMin, i) - XMVectorGetByIndex(segStart, i)) /
          XMVectorGetByIndex(dir, i);
      float tMax =
          (XMVectorGetByIndex(aabbMax, i) - XMVectorGetByIndex(segStart, i)) /
          XMVectorGetByIndex(dir, i);
      if (tMin > tMax) std::swap(tMin, tMax);
      t = imath::Clamp(t, tMin, tMax);
    }
  }

  return segStart + dir * t;
}

void physics_math::ClosestPointBetweenSegments(
    IVec a1, IVec a2, IVec b1, IVec b2, IVec& closestA, IVec& closestB) {
  // 实现参考Real-Time Collision Detection
  IVec d1 = a2 - a1;
  IVec d2 = b2 - b1;
  IVec r = a1 - b1;

  float a = XMVectorGetX(XMVector3Dot(d1, d1));
  float e = XMVectorGetX(XMVector3Dot(d2, d2));
  float f = XMVectorGetX(XMVector3Dot(d2, r));

  // 处理退化线段
  if (a <= FLT_EPSILON && e <= FLT_EPSILON) {
    closestA = a1;
    closestB = b1;
    return;
  }

  if (a <= FLT_EPSILON) {
    closestA = a1;
    closestB = b1 + imath::Clamp(-f / e, 0.0f, 1.0f) * d2;
    return;
  }

  float c = XMVectorGetX(XMVector3Dot(d1, r));
  if (e <= FLT_EPSILON) {
    closestA = a1 + imath::Clamp(-c / a, 0.0f, 1.0f) * d1;
    closestB = b1;
    return;
  }

  // 计算参数
  float b = XMVectorGetX(XMVector3Dot(d1, d2));
  float denom = a * e - b * b;

  float s = (denom != 0.0f) ? imath::Clamp((b * f - c * e) / denom, 0.0f, 1.0f)
                            : 0.0f;
  float t = (b * s + f) / e;

  if (t < 0.0f) {
    t = 0.0f;
    s = imath::Clamp(-c / a, 0.0f, 1.0f);
  } else if (t > 1.0f) {
    t = 1.0f;
    s = imath::Clamp((b - c) / a, 0.0f, 1.0f);
  }

  closestA = a1 + s * d1;
  closestB = b1 + t * d2;
}

} // namespace ifire::game